#ifndef _DISPATCH_SERVER_H_
#define _DISPATCH_SERVER_H_

#include <memory>
#include <mutex>
#include <list>
#include <string.h>
#include <iostream>
#include <cmath>

#include <dispatch_server/global.h>
#include <moying_proto/moying_proto.h>
#include <moying_comm_lib/reply.h>
#include <moying_comm_lib/publish.h>
#include <dispatch_system/time_based_astar.h>
#include <dispatch_system/moying_map.h>
#include <commu_with_sm/system_manager_commu.hpp>

#include <schedule_config/config.hpp>
#include <dispatch_system_ros_viz/moyingmap_viz.h>
#include <dispatch_system_ros_viz/vehicle_viz.h>
// #include <dispatch_server/traffic_monitor.h>
#include <dispatch_server/vehicle_monitor.h>
#include <moying_proto/moying_proto.h>

using namespace moying::sch;
using namespace moying_proto;

namespace moying{

namespace navigation{
    class DeadLockHandler;
}

namespace communication
{



class DispatchServer;
typedef std::shared_ptr<DispatchServer> DispatchServerPtr;


struct PathPlanningRequest
{
    PathPlanningRequest(){}
    PathPlanningRequest(const navigation::VehiclePtr _vehicle, const navigation::VehicleState _start, const navigation::VehicleState _goal
                    ,int _path_plan_start_time=0)
    :vehicle(_vehicle), start(_start), goal(_goal),path_plan_start_time(_path_plan_start_time)
    {

    }
    navigation::VehiclePtr vehicle; //机器人对象
    navigation::VehicleState start; //起始点
    navigation::VehicleState goal;  //目标点
    bool is_global_goal = true;            //目标点是否全局目标点,还是临时的一个局部目标点,在机器人死锁避让时,机器人会导航到一个临时目标点
    bool is_query_from_outside = false;     //是否是从外部发来的路径规划请求
    bool is_revise_path = false;        //是否是修正路径引起的重规划请求
    SearchCellArea search_area;     //路径规划时的限制搜索区域
    time_t path_plan_start_time = 0;      //开始路径规划的时间,0:立即规划,大于0:当前时间超过该值的时间后,才开始规划,1970年到目前的秒数
    unsigned int occupy_time_after_arrive=0;  //到达目标点后持续占用时间地的秒数

};

//固定导航路径数据
struct ConstNavPathData{
    std::vector<moying_proto::ConstantPathInfo> all_path;
};

class DispatchServer{

public:

    //生成单例对象
    static DispatchServerPtr instance();

    /** 初始化
     * TODO: 从config导入初始化参数
     * @param ws    当前使用的地图工作空间名,如果为空,从配置文件读取
     * @param map   当前使用的地图名,如果为空,从配置文件读取
     */
    bool initialize(std::string ws="",std::string map="");

    /**
     * @brief run    启动通信线程
     * @param req_port      交通管理应答的端口
     * @param sub_port      交通管理推送的端口
     * @return             成功或失败
     */
    bool run();

    // moying_proto::SwitchMapTMService_Request switch_map_req(){return switch_map_req_;}

    void handlePathPlanService(const moying_proto::PathPlanService_Request& req, moying_proto::PathPlanService_Response& res);
    void handleCancelNavPathService(const moying_proto::CancelNavService_Request& req,moying_proto::CancelNavService_Response& res);
    void handleAllPathPlanService(const moying_proto::GetAllRobotPathDataService_Request& req, moying_proto::GetAllRobotPathDataService_Response& res);
    void handleSwitchMapTMService(const moying_proto::SwitchMapTMService_Request& req,moying_proto::SwitchMapTMService_Response& res);
    void handleOccupyTimeMapService(const moying_proto::OccupyTimeMapService_Request& req,moying_proto::OccupyTimeMapService_Response& res);
    void handleWhetherPoseInOneWayRoadService(const moying_proto::WhetherPoseInOneWayRoadService_Request& req,
            moying_proto::WhetherPoseInOneWayRoadService_Response& res);
    void handleManageConstNavPathService(const moying_proto::ManageConstantNavPathService_Request& req,
            moying_proto::ManageConstantNavPathService_Response& res);
    
    void notifyMapSwitchResult(std::string ws,std::string map,bool switch_result);
    void notifyPlanResult(unsigned int robot_id, unsigned int result);
    void notifyPathPlanData(const std::vector<moying_proto::PathPoint>& points, unsigned int robot_id);
    void notifyRobotCannotArrive(const moying_proto::RobotCannotArriveDestNotify& notify);
    // void notifyFollowingPath(const std::vector<moying_proto::PathPoint>& points,IdType robot_id);

    void navConstantPath(const moying_proto::PathPlanService_Request& req, moying_proto::PathPlanService_Response& res);

    void delayMs2delays(int delay_ms, int& delay_s);

    void startPlanningThread();

    bool addVehicle(const RobotPtr &robot);
    bool addVehicle(unsigned int id, std::string type,moying_proto::NavBaseType base_type, const std::string &name, double x, double y, double theta);

    bool deleteVehicle(unsigned int id);

    void showPathPoint(unsigned int id,navigation::PlanningResult result, std::vector<moying_proto::PathPoint>& points);

    void stateToPathPoint(const VehicleState &state, PathPoint &point,int point_index,int point_property);

    void getProtoPath(const navigation::PlanningResult& result,std::vector<moying_proto::PathPoint>& path);

    void robotStateToPathPoint(const VehicleState &state, moying_proto::PathPoint &point, int index);

    // bool handleStepArrived(unsigned int robot_id, unsigned int reached_index, unsigned long long reached_time);

    bool getPathResult(unsigned int robot_id, navigation::PlanningResult& path_result);

    void showResultTime(unsigned int robot_id);

    void robotPoseNotifyCB (const moying_proto::RobotPoseNotify& notify);
    void robotOnoffLineNotifyCB (const moying_proto::RobotOnOffLineNotify& notify);
    void robotNavStateNotifyCB (const moying_proto::NavStateNotify& notify);
    // void localCostmapNotifyCB(const moying_proto::LocalCostMapNotify& notify);
    void localObstacleNotifyCB(const moying_proto::LocalObstacleNotify& notify);
    void robotSizeUpdateNotify(const moying_proto::RobotSizeUpdateNotify& notify);
    void mapFileDataUpdateNotify(const moying_proto::MapFileDataUpdateNotify& notify);
    

    //把路径规划请求加入到队列中
    bool addPathPlanRequest(unsigned int robot_id,const PathPlanningRequest& req);

    //更新机器人路径规划结果,机器人路径点到达时刻更新时,会调用这个函数
    bool updateVehiclePathResult(unsigned int robot_id,const PlanningResult& result);

    /**
     * @brief 初始化路径规划
     * @param ws    路径规划使用的地图工作空间
     * @param map   路径规划使用的地图 
     * @return  切换成功或失败
     */ 
    bool initPathPlan(std::string ws,std::string map);

    navigation::MoyingMapPtr moyingmap(){return moyingmap_;}

    /**
     * @brief 对没有路径的机器人在当前点,从当前时该占用指定时间的时间地图
     * @param vehicle_id    机器人id
     * @param seconds       占用的秒数 
     * @return  成功或失败
     */
    bool noPathVehicleOccupyTimemap(unsigned int vehicle_id,int seconds);

    /**
     * @brief 处理发送停止导航命令给机器人
     * @param vehicle_id    机器人id
     * @param seconds       占用的秒数 
     * @return  成功或失败
     */
    void handleSendStopToRobot(IdType robot_id);
    
    //订阅系统管理通知
    void subSystemManageNotifies();

    bool loadConstNavPathData();
    bool saveConstNavPathData();
    bool getConstPath(std::string map_workspace,std::string map_name,std::string path_id,
        std::vector<moying_proto::ConstantPathInfo>& const_path_vec);
private:
    //通信库的封包日志函数
    bool packLogFunc(const std::string& msg);

private:
    static DispatchServerPtr ptr_;
    static std::mutex singleton_mutex_;

    moying::communication::MoyingCommuPublishPtr      publisher_      = nullptr;      //推送器
    moying::communication::MoyingCommuReplyPtr        replier_        = nullptr;      //应答器

    navigation::MoyingMapPtr moyingmap_ = nullptr;
    navigation::TimeBasedAStarPtr astar_= nullptr;
    
    // std::shared_ptr<TrafficMonitor> traffic_monitor_ptr_;

    visualization::MoyingMapVizPtr map_viz_;
    
    std::mutex time_map_mutex_; //时间地图同步
    std::mutex planning_queue_mutex_;
    std::recursive_mutex vehicle_mutex_;
    std::map<unsigned int, visualization::VehicleVizPtr> vehicle_viz_;
    std::map<unsigned int, navigation::VehicleMonitorPtr> vehicle_monitors_;
    // std::map<unsigned int, std::pair<navigation::PlanningResult, int>> vehicle_paths_;  //所有机器人路径规划结果,值的第一个字段为规划结果,第二个字段为当前发送的路径点索引
    std::list<std::pair<unsigned int, PathPlanningRequest>> planning_queue_;
    
    std::chrono::system_clock::time_point start_time_;
    std::chrono::system_clock::time_point current_update_time_;
    std::chrono::system_clock::time_point next_update_time_;
    unsigned int sec_since_start_;

    DeadLockHandler* deadlock_handler_ = nullptr;

    // moying_proto::SwitchMapTMService_Request switch_map_req_;   //地图切换请求,如果ws和map都为空不切换
    bool is_updating_map_data_ = false;  //是否正在更新地图
    bool path_plan_thread_is_running_ = false; //路径规划线程是否正在运行
    bool stop_path_plan_thread_ = false;   //是否结束路径规划线程

    std::recursive_mutex data_mutex_;      //同步对数据的操作
    ConstNavPathData all_const_nav_path_;
};

} // namespace communication
} // namespace moying
#endif //_DISPATCH_SERVER_H_